﻿using System;
using System.Collections.Generic;
using System.IO;

namespace Raven
{
    public enum SearchType
    {
        AStar,
        Dijkstra
    };

    public abstract class GraphSearchTimeSliced<Edge>
    {
        private SearchType m_SearchType;


        public GraphSearchTimeSliced(SearchType type)
        {
            m_SearchType = type;
        }

        //When called, this method runs the algorithm through one search cycle. The
        //method returns an enumerated value (target_found, target_not_found,
        //search_incomplete) indicating the status of the search
        public abstract PathSearch CycleOnce();

        //returns the vector of edges that the algorithm has examined
        public abstract List<Edge> GetSPT();


        //returns the total cost to the target
        public abstract Fix GetCostToTarget();

        //returns a list of node indexes that comprise the shortest path
        //from the source to the target
        public abstract LinkedList<int> GetPathToTarget();

        //returns the path as a list of PathEdges
        public abstract LinkedList<PathEdge> GetPathAsPathEdges();

        public SearchType GetSearchType()
        {
            return m_SearchType;
        }
    }

    public class GraphSearchAStarTS : GraphSearchTimeSliced<NavGraphEdge>
    {
        private SparseGraph<NavGraphNode<Trigger<RavenBot>>, NavGraphEdge> _graph;
        private Fix[] _gCosts;
        private Fix[] _fCost;

        private NavGraphEdge[] _shortestPathTree;
        private NavGraphEdge[] _searchFrontier;

        private int _source;
        private int _target;

        IndexedPriority<Fix> _pQ;

        public GraphSearchAStarTS(SparseGraph<NavGraphNode<Trigger<RavenBot>>, NavGraphEdge> graph, int source,
            int target) : base(SearchType.AStar)
        {
            _graph = graph;
            _shortestPathTree = new NavGraphEdge[_graph.NumNodes()];
            _searchFrontier = new NavGraphEdge[_graph.NumNodes()];
            _gCosts = new Fix[_graph.NumNodes()];
            _fCost = new Fix[_graph.NumNodes()];
            _source = source;
            _target = target;
            //create the PQ   
            _pQ = new IndexedPriority<Fix>(_fCost, Comparer<Fix>.Default, _graph.NumNodes());

            //put the source node on the queue
            _pQ.Insert(_source);
        }

        public override PathSearch CycleOnce()
        {
            //if the PQ is empty the target has not been found
            if (_pQ.Empty())
            {
                return PathSearch.target_not_found;
            }

            //get lowest cost node from the queue
            int NextClosestNode = _pQ.Pop();

            //put the node on the SPT
            _shortestPathTree[NextClosestNode] = _searchFrontier[NextClosestNode];

            //if the target has been found exit
            if (NextClosestNode == _target)
            {
                return PathSearch.target_found;
            }

            //now to test all the edges attached to this node
            foreach (var pE in _graph.GetEdges(NextClosestNode))
            {
                //calculate the heuristic cost from this node to the target (H)                       
                Fix HCost = Calculate(_target, pE.To());

                //calculate the 'real' cost to this node from the source (G)
                Fix GCost = _gCosts[NextClosestNode] + pE.Cost();

                //if the node has not been added to the frontier, add it and update
                //the G and F costs
                if (_searchFrontier[pE.To()] == null)
                {
                    _fCost[pE.To()] = GCost + HCost;
                    _gCosts[pE.To()] = GCost;

                    _pQ.Insert(pE.To());

                    _searchFrontier[pE.To()] = pE;
                }

                //if this node is already on the frontier but the cost to get here
                //is cheaper than has been found previously, update the node
                //costs and frontier accordingly.
                else if ((GCost < _gCosts[pE.To()]) && (_shortestPathTree[pE.To()] == null))
                {
                    _fCost[pE.To()] = GCost + HCost;
                    _gCosts[pE.To()] = GCost;

                    _pQ.ChangePriority(pE.To());

                    _searchFrontier[pE.To()] = pE;
                }
            }

            //there are still nodes to explore
            return PathSearch.search_incomplete;
        }

        private Fix Calculate(int nd1, int nd2)
        {
            return (_graph.GetNode(nd1).Pos() - _graph.GetNode(nd2).Pos()).magnitude;
        }

        public override List<NavGraphEdge> GetSPT()
        {
            return new List<NavGraphEdge>(_shortestPathTree);
        }

        public override Fix GetCostToTarget()
        {
            return _gCosts[_target];
        }

        public override LinkedList<int> GetPathToTarget()
        {
            LinkedList<int> path = new LinkedList<int>();

            //just return an empty path if no target or no path found
            if (_target < 0)
            {
                return path;
            }

            int nd = _target;

            path.AddLast(nd);

            while (nd != _source && _shortestPathTree[nd] != null)
            {
                nd = _shortestPathTree[nd].From();

                path.AddFirst(nd);
            }

            return path;
        }

        public override LinkedList<PathEdge> GetPathAsPathEdges()
        {
            LinkedList<PathEdge> path = new LinkedList<PathEdge>();

            //just return an empty path if no target or no path found
            if (_target < 0)
            {
                return path;
            }

            int nd = _target;

            while (nd != _source && _shortestPathTree[nd] != null)
            {
                path.AddFirst(new PathEdge(_graph.GetNode(_shortestPathTree[nd].From()).Pos(),
                    _graph.GetNode(_shortestPathTree[nd].To()).Pos(),
                    0,
                    0));

                nd = _shortestPathTree[nd].From();
            }

            return path;
        }
    }
    
    public class GraphSearchDijkstrasTS : GraphSearchTimeSliced<NavGraphEdge>
        {
            private SparseGraph<NavGraphNode<Trigger<RavenBot>>, NavGraphEdge> _graph;
            private Fix[] _costToThisNode;

            private NavGraphEdge[] _shortestPathTree;
            private NavGraphEdge[] _searchFrontier;

            public delegate bool SatisfiedFunc(int nodeIndex);
            private SatisfiedFunc _satisfied;
            private int _source;
            private int _target;

            IndexedPriority<Fix> _pQ;

            public GraphSearchDijkstrasTS(SparseGraph<NavGraphNode<Trigger<RavenBot>>, NavGraphEdge> graph, int source,
                SatisfiedFunc satisfied) : base(SearchType.AStar)
            {
                _graph = graph;
                _shortestPathTree = new NavGraphEdge[_graph.NumNodes()];
                _searchFrontier = new NavGraphEdge[_graph.NumNodes()];
                _costToThisNode = new Fix[_graph.NumNodes()];
                _source = source;
                _target = 0;
                _satisfied = satisfied;
                _pQ = new IndexedPriority<Fix>(_costToThisNode, Comparer<Fix>.Default,
                    _graph.NumNodes());
                _pQ.Insert(_source);
            }

            public override PathSearch CycleOnce()
            {
                //if the PQ is empty the target has not been found
                if (_pQ.Empty())
                {
                    return PathSearch.target_not_found;
                }

                //get lowest cost node from the queue
                int NextClosestNode = _pQ.Pop();

                //move this node from the frontier to the spanning tree
                _shortestPathTree[NextClosestNode] = _searchFrontier[NextClosestNode];

                //if the target has been found exit
                if (_satisfied(NextClosestNode))
                {
                    //make a note of the node index that has satisfied the condition. This
                    //is so we can work backwards from the index to extract the path from
                    //the shortest path tree.
                    _target = NextClosestNode;

                    return PathSearch.target_found;
                }

                //now to test all the edges attached to this node
                foreach (var pE in _graph.GetEdges(NextClosestNode))
                {
                    //the total cost to the node this edge points to is the cost to the
                    //current node plus the cost of the edge connecting them.
                    Fix NewCost = _costToThisNode[NextClosestNode] + pE.Cost();

                    //if this edge has never been on the frontier make a note of the cost
                    //to get to the node it points to, then add the edge to the frontier
                    //and the destination node to the PQ.
                    if (_searchFrontier[pE.To()] == null)
                    {
                        _costToThisNode[pE.To()] = NewCost;

                        _pQ.Insert(pE.To());

                        _searchFrontier[pE.To()] = pE;
                    }

                    //else test to see if the cost to reach the destination node via the
                    //current node is cheaper than the cheapest cost found so far. If
                    //this path is cheaper, we assign the new cost to the destination
                    //node, update its entry in the PQ to reflect the change and add the
                    //edge to the frontier
                    else if (NewCost < _costToThisNode[pE.To()] &&
                             _shortestPathTree[pE.To()] == null)
                    {
                        _costToThisNode[pE.To()] = NewCost;

                        //because the cost is less than it was previously, the PQ must be
                        //re-sorted to account for this.
                        _pQ.ChangePriority(pE.To());

                        _searchFrontier[pE.To()] = pE;
                    }
                }

                //there are still nodes to explore
                return PathSearch.search_incomplete;
            }

            public override List<NavGraphEdge> GetSPT()
            {
                return new List<NavGraphEdge>(_shortestPathTree);
            }

            public override Fix GetCostToTarget()
            {
                return _costToThisNode[_target];
            }

            public override LinkedList<int> GetPathToTarget()
            {
                LinkedList<int> path = new LinkedList<int>();

                //just return an empty path if no target or no path found
                if (_target < 0) return path;

                int nd = _target;

                path.AddLast(nd);

                while ((nd != _source) && (_shortestPathTree[nd] != null))
                {
                    nd = _shortestPathTree[nd].From();

                    path.AddFirst(nd);
                }

                return path;
            }

            public override LinkedList<PathEdge> GetPathAsPathEdges()
            {
                LinkedList<PathEdge> path = new LinkedList<PathEdge>();

                //just return an empty path if no target or no path found
                if (_target < 0) return path;

                int nd = _target;

                while ((nd != _source) && (_shortestPathTree[nd] != null))
                {
                    path.AddFirst(new PathEdge(_graph.GetNode(_shortestPathTree[nd].From()).Pos(),
                        _graph.GetNode(_shortestPathTree[nd].To()).Pos(),
                        0,
                        0));

                    nd = _shortestPathTree[nd].From();
                }

                return path;
            }
        }
}